/*
 * @Description: 
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:11:30
 * @LastEditTime: 2021-10-26 21:17:56
 * @LastEditors: Sang Hao
 */

#include "lidar_slam/subscriber/key_frame_subscriber.hpp"

namespace lidar_slam {

/**
 * @description: 订阅话题，注册回调函数
 * @param  {*}
 * @return {*}
 */
KeyFrameSubscriber::KeyFrameSubscriber(
	ros::NodeHandle nh, std::string topic_name, size_t buff_size) 
	:nh_(nh){
	subscriber_ = nh_.subscribe(topic_name, buff_size, &KeyFrameSubscriber::msg_callback, this);

}
/**
 * @description: 回调函数，其参数是指向发布话题的指针，包装进了其他类里
 * @param  {*}
 * @return {*}
 */
void KeyFrameSubscriber::msg_callback(
	const geometry_msgs::PoseWithCovarianceStampedConstPtr& key_frame_msg_ptr) {
	KeyFrame key_frame;
	key_frame.time = key_frame_msg_ptr->header.stamp.toSec();
	// 这个index存在了方差里边
	key_frame.index = (unsigned int)key_frame_msg_ptr->pose.covariance[0];
	
	key_frame.pose(0, 3) = key_frame_msg_ptr->pose.pose.position.x;
	key_frame.pose(1, 3) = key_frame_msg_ptr->pose.pose.position.y;
	key_frame.pose(2, 3) = key_frame_msg_ptr->pose.pose.position.z;

	Eigen::Quaternionf q;
	q.x() = key_frame_msg_ptr->pose.pose.orientation.x;
	q.y() = key_frame_msg_ptr->pose.pose.orientation.y;
	q.z() = key_frame_msg_ptr->pose.pose.orientation.z;
	q.w() = key_frame_msg_ptr->pose.pose.orientation.w;
	key_frame.pose.block<3, 3>(0, 0) = q.matrix();

	new_key_frame_.push_back(key_frame);
}

/**
 * @description: 解析数据，也就是把新订阅的new_key_frame_加到key_frame_buff里边
 * @param  {*}
 * @return {*}
 */
void KeyFrameSubscriber::ParseData(std::deque<KeyFrame>& key_frame_buff) {
	if (new_key_frame_.size() > 0) {
		key_frame_buff.insert(key_frame_buff.end(), new_key_frame_.begin(), new_key_frame_.end());
		new_key_frame_.clear();
	}
}
}